package crap;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.MouseAdapter;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.geom.Path2D;
import java.awt.geom.Point2D;
import java.awt.image.BufferedImage;

import javax.imageio.ImageIO;
import javax.swing.JComponent;
import javax.swing.JFrame;

import org.ejml.simple.SimpleMatrix;

public class CTester04 {
	public static SimpleMatrix crossProduct3x3(SimpleMatrix vecA, SimpleMatrix vecB){
		SimpleMatrix A = new SimpleMatrix(new double[][]{
				{0,				-vecA.get(2),	vecA.get(1)},
				{vecA.get(2),	0,				-vecA.get(0)},
				{-vecA.get(1),	vecA.get(0),	0}
		});
		return A.mult(vecB);
	}
	
	public static void main(String[] args) throws Exception {
		final BufferedImage bi = ImageIO.read(CTester04.class.getClassLoader().getResourceAsStream("res/P5100208s.JPG"));
		final double mw = 320, mh = 240;
		double scale = 0.038*640/0.0173;
//		final Point2D[] camPoints = {new Point2D.Double(400-mw,160-mh), new Point2D.Double(464-mw,344-mh), new Point2D.Double(167-mw,393-mh), new Point2D.Double(119-mw, 191-mh)};
		final Point2D[] camPoints = {new Point2D.Double(428-mw,174-mh), new Point2D.Double(568-mw,301-mh), new Point2D.Double(185-mw,377-mh), new Point2D.Double(88-mw, 222-mh)};
		final Point2D[] A4Points = {new Point2D.Double(0, 0), new Point2D.Double(0.210, 0), new Point2D.Double(0.210, 0.297), new Point2D.Double(0, 0.297)};
		final Path2D markerLine = new Path2D.Double();

		markerLine.moveTo(camPoints[0].getX()+mw, camPoints[0].getY()+mh);
		markerLine.lineTo(camPoints[1].getX()+mw, camPoints[1].getY()+mh);
		markerLine.lineTo(camPoints[2].getX()+mw, camPoints[2].getY()+mh);
		markerLine.lineTo(camPoints[3].getX()+mw, camPoints[3].getY()+mh);
		markerLine.closePath();
		
		JFrame ff = new JFrame();
		ff.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		
		final JComponent canvas = new JComponent() {
			{setPreferredSize(new Dimension(bi.getWidth(), bi.getHeight()));}
			@Override
			public void paint(Graphics g) {
				Graphics2D g2 = (Graphics2D)g;
				g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
				g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL, RenderingHints.VALUE_STROKE_PURE);
				g2.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
				
				g2.drawImage(bi, 0, 0, this);
				g2.setStroke(new BasicStroke(2f));
				g2.setColor(Color.red);
				g2.draw(markerLine);
				
				super.paint(g);
			}
		};
		
		MouseListener ml = new MouseAdapter() {
			@Override
			public void mouseClicked(MouseEvent e) {
				System.out.println(e.getPoint());
				super.mouseClicked(e);
			}
		};
		canvas.addMouseListener(ml);
		
		SimpleMatrix leftSide = new SimpleMatrix(8, 8), rightSide = new SimpleMatrix(8, 1);
		
		
		
		double x0=A4Points[0].getX(),x1=A4Points[1].getX(),x2=A4Points[2].getX(),x3=A4Points[3].getX();
		double y0=A4Points[0].getY(),y1=A4Points[1].getY(),y2=A4Points[2].getY(),y3=A4Points[3].getY();
		double x0__=camPoints[0].getX(),x1__=camPoints[1].getX(),x2__=camPoints[2].getX(),x3__=camPoints[3].getX();
		double y0__=camPoints[0].getY(),y1__=camPoints[1].getY(),y2__=camPoints[2].getY(),y3__=camPoints[3].getY();
		
		double[][] dataLeft = { // x_ und y_ sind x" und y", x und y sind x0 und y0
				{x0,y0,1, 	0,0,0, 		-x0__*x0, -x0__*y0},
				{0,0,0,		x0,y0,1,	-y0__*x0, -y0__*y0},
				{x1,y1,1,	0,0,0,		-x1__*x1, -x1__*y1},
				{0,0,0,		x1,y1,1,	-y1__*x1, -y1__*y1},
				{x2,y2,1,	0,0,0,		-x2__*x2, -x2__*y2},
				{0,0,0,		x2,y2,1,	-y2__*x2, -y2__*y2},
				{x3,y3,1,	0,0,0,		-x3__*x3, -x3__*y3},
				{0,0,0,		x3,y3,1,	-y3__*x3, -y3__*y3}
		};

		double[][] dataRight = {{x0__},{y0__},{x1__},{y1__},{x2__},{y2__},{x3__},{y3__}};
		SimpleMatrix Left = new SimpleMatrix(dataLeft); // 8x8 matrix
		SimpleMatrix Right = new SimpleMatrix(dataRight); // 8x1 matrix
		
		
		SimpleMatrix myH = Left.invert().mult(Right);
		SimpleMatrix mymyH = new SimpleMatrix(3,3);
		for (int i=0;i<myH.getNumElements();i++){ mymyH.set(i, myH.get(i));	}
		mymyH.set(8, 1);
		
		// build R
		// R := [ |h1| |h2| |h1|x|h2| ]
		
		double mylambda1 = mymyH.extractVector(false, 0).normF();
		System.out.println("mymH: "+mymyH.extractVector(false, 0));
		double mylambda2 = mymyH.extractVector(false, 1).normF();
		System.out.println("l1: "+mylambda1+" :: l2: "+mylambda2+" :: delta: "+Math.abs(mylambda2-mylambda1));
		// groesseres lambda auswaehlen
		double mylambda = Math.max(mylambda1, mylambda2);
		System.out.println("mylambda: "+mylambda);
		
		// spalten normieren
		SimpleMatrix myh1 = mymyH.extractVector(false, 0).divide(mylambda);
		SimpleMatrix myh2 = mymyH.extractVector(false, 1).divide(mylambda);
		SimpleMatrix myh3 = crossProduct3x3(myh1, myh2);
		
		// in R eintragen
		SimpleMatrix myR = new SimpleMatrix(3,3);
		myR.insertIntoThis(0, 0, myh1);
		myR.insertIntoThis(0, 1, myh2);
		myR.insertIntoThis(0, 2, myh3.scale(-1));
		System.out.println(myR);
		
		// myh3 = -Rt
		SimpleMatrix myRinverse = myR.invert();
		System.out.println("myRinverse :: \012"+myRinverse);
		SimpleMatrix t = myR.scale(-1).invert().mult(mymyH.extractVector(false, 2).divide(mylambda));
		System.out.println("ttttttt = \012"+t);
		System.out.println("scaledTTTTTT =\012"+t.scale(scale));
		
		SimpleMatrix myT = myR.scale(-1).invert().mult(mymyH.divide(mylambda).extractVector(false, 2));
		System.out.println("myT: "+myT);
		
		System.out.println("\012------>MEGAMATRIX<------------\012");
		System.out.println(Left);
		System.out.println(Right);
		System.out.println(myH);
		System.out.println(mymyH);
		System.out.println("\012------>END MEGAMATRIX<------------\012");
		
		
		
		
		
		for (int i = 0; i<4; i++) {
			
			double x = A4Points[i].getX(), y = A4Points[i].getY(), x_ = camPoints[i].getX(), y_ = camPoints[i].getY();
			
			//x_ = (x y 1 0 0 0 -x_*x -x_*y ) h = x_
			leftSide.set(i*2, 0, x);
			leftSide.set(i*2, 1, y);
			leftSide.set(i*2, 2, 1);

			leftSide.set(i*2, 3, 0);
			leftSide.set(i*2, 4, 0);
			leftSide.set(i*2, 5, 0);
			
			leftSide.set(i*2, 6, -x_*x);
			leftSide.set(i*2, 7, -x_*y);
			
			//y_ = (0 0 0 x y 1 -y_*x -y_*y) h = y_
			leftSide.set(i*2+1, 0, 0);
			leftSide.set(i*2+1, 1, 0);
			leftSide.set(i*2+1, 2, 0);

			leftSide.set(i*2+1, 3, x);
			leftSide.set(i*2+1, 4, y);
			leftSide.set(i*2+1, 5, 1);
			
			leftSide.set(i*2+1, 6, -y_*x);
			leftSide.set(i*2+1, 7, -y_*y);
			
			// x_ und y_ im Ergebnisvektor setzen 
			rightSide.set(i*2, 0, x_);
			rightSide.set(i*2+1, 0, y_);
		}
		
		System.out.println(leftSide);
		System.out.println(rightSide);
		
//		System.out.println(leftSide.svd().getW());
//		System.out.println(leftSide.invert().mult(rightSide));
		SimpleMatrix h = leftSide.solve(rightSide);
		System.out.println("h: "+h);
		double h11 = h.get(0, 0), h12 = h.get(1,0), h13 = h.get(2, 0), h21 = h.get(3, 0), h22 = h.get(4, 0), h23 = h.get(5, 0), h31 = h.get(6, 0), h32 = h.get(7, 0);
		double lambda1 = Math.sqrt(h11*h11+h21*h21+h31*h31), lambda2 = Math.sqrt(h12*h12+h22*h22+h32*h32);
		
		System.out.println("lambda1: "+lambda1+"   lambda2: "+lambda2+" delta: "+(lambda1-lambda2));
		
		double lambda = Math.max(lambda1, lambda2);
		double[][] hma = {{h11, h12, h13},{h21, h22, h23},{h31, h32, 1}};

		h11/=lambda;
		h12/=lambda;
		h13/=lambda;
		h21/=lambda;
		h22/=lambda;
		h23/=lambda;
		h31/=lambda;
		h32/=lambda;
		
		
		
//		h11*=scale;
//		h12*=scale;
//		h13*=scale;
//		h21*=scale;
//		h22*=scale;
//		h23*=scale;
//		h31*=scale;
//		h32*=scale;
		
		double[][] rma = {{h11, h12, -(h21*h32-h22*h31)},{h21, h22, -(h31*h12-h32*h11)},{h31, h32, -(h11*h22-h21*h12)}};
		double[][] kma = {{scale, 0, 0},{0, scale, 0},{0, 0, 1}};
		
		SimpleMatrix H = new SimpleMatrix(hma);
		System.out.println("H unscaled: "+H);
		H = H. scale(1/lambda);
		System.out.println("mymyH: ");
		// H scalen mit mylambda
		System.out.println(mymyH.divide(mylambda));

		SimpleMatrix K = new SimpleMatrix(kma);
		
		System.out.println("h11: "+h11+" h12: "+h12+" h13: "+h13+" h21: "+h21+" h22: "+h22+" h23: "+h23+" h31: "+h31+" h32: "+h32);
		
		SimpleMatrix result = new SimpleMatrix(4,4), R = new SimpleMatrix(rma), h3_ = new SimpleMatrix(3,1);
		
		Point2D oo = camPoints[2];
		SimpleMatrix p__ = new SimpleMatrix(new double[][]{{oo.getX()*scale}, {oo.getY()*scale}, {1}});
		
		SimpleMatrix mapping = H.invert().mult(K.invert());
		
		h3_.set(0, 0, h13);
		h3_.set(1, 0, h23);
		h3_.set(2, 0, 1/lambda);
		
		System.out.println("H: ");
		System.out.println(H);
		System.out.println();
		System.out.println("mapping: ");
		System.out.println(mapping);
		System.out.println();
		System.out.println("R: ");
		System.out.println(R);
		System.out.println();
		System.out.println("H3: ");
		System.out.println(h3_);
		System.out.println("t: ");
		System.out.println(R.scale(-1).invert().mult(h3_));
//		System.out.println(h);
		SimpleMatrix poopoo = mapping.mult(p__);
		poopoo = poopoo.scale(1/poopoo.get(2,0));
		System.out.println(poopoo);
		
		ff.add(canvas);
		
		ff.pack();
		ff.setVisible(true);
		
	}
}
